package com.xexun.gpstracker.indoorlocation;

import android.graphics.Point;
import android.util.Log;
import com.xexun.gpstracker.common.Constants;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;

public class IndoorNavigation {
    private List<Node> open;
    private List<Node> close;

    public IndoorNavigation() {
        open = new ArrayList<Node>();
        close = new ArrayList<Node>();
    }

    public List<Integer> navigation(int start, int end) {    //start,end对应MapPoint中数组下标
        int size = MapPoint.points.size();
        if(start <= 0 || start >= size
            || end <= 0 || end >= size) {
            return null;
        }

        Node startNode = new Node(null,0,MapPoint.points.get(start).point,start);
        close.add(startNode);

        int index;
        Point father;
        Point current;
        Point destPoint = MapPoint.points.get(end).point;
        Node curNode;
        boolean find = false;
        //找与start相连的点
        for (int[] line : MapLine.lineArray) {
            if (line[0] == start) {
                index = line[1];
                current = MapPoint.points.get(index).point;
            } else if (line[1] == start) {
                index = line[0];
                current = MapPoint.points.get(index).point;
            } else {
                continue;
            }
            Log.i(Constants.TAG, "找到与开始点"+start+"相连的点："+index);
            father = MapPoint.points.get(start).point;
            Node node = new Node(father, start, current, index);
            double d = getDistance(father, current);
            node.setGValue(d);
            double h = getHuffmanDistance(current,destPoint);
            node.setHValue(h);
            Log.i(Constants.TAG, "设置其父结点"+start+",g:"+Double.toString(d)+",h:"+(int)(h));
            open.add(node);

            if(end == index) {
                Log.i(Constants.TAG, "找到终点");
                find = true;
                break;
            }
        }

        do {
            //寻找开启列表中F值最低的点
            Collections.sort(open, new Comparator<Node>() {  //将绝对值排序
                @Override
                public int compare(Node lhs, Node rhs) {
                    return ((Integer) ((int)(lhs.hValue+lhs.gValue))).compareTo((Integer) ((int)(rhs.hValue+rhs.gValue)));
                }
            });
            curNode = open.get(0);
            Log.i(Constants.TAG, "找到F值最小的点："+curNode.index);
            close.add(open.get(0));
            open.remove(0);

            int findIndex = 0;
            //找与curNode相连的点
            int curIndex = curNode.index;

            for(int i=0; i<MapLine.lineArray.length; i++) {
                int[] line = MapLine.lineArray[i];
                if (line[0] == curIndex) {
                    index = line[1];
                    current = MapPoint.points.get(index).point;
                } else if (line[1] == curIndex) {
                    index = line[0];
                    current = MapPoint.points.get(index).point;
                } else {
                    continue;
                }
                Log.i(Constants.TAG, "找到与当前点"+curIndex+"相连的点："+index);

                boolean isInClose = false;
                //判断找到的点是否在close列表中
                for(Node nd : close) {
                    if(nd.index == index) {
                        isInClose = true;
                        break;
                    }
                }
                if(isInClose) {
                    Log.i(Constants.TAG,index+"点已经在close表中");
                    continue;
                }

                boolean isInOpen = false;
                int inOpenIndex = 0;
                //判断找到的点是否在open列表中
                for(Node nd : open) {
                    if(nd.index == index) {
                        isInOpen = true;
                        Log.i(Constants.TAG,index+"点在open表中");
                        break;
                    }
                    inOpenIndex++;
                }
                father = MapPoint.points.get(curIndex).point;
                if(!isInOpen) { //不在open中就加入
                    Log.i(Constants.TAG, "加入open表中");
                    Node node = new Node(father, curIndex, current, index);
                    double d = curNode.gValue + getDistance(father, current);
                    node.setGValue(d);
                    double h = getHuffmanDistance(current,destPoint);
                    node.setHValue(h);
                    open.add(node);
                    Log.i(Constants.TAG, "设置其父结点"+curIndex+",g:"+Double.toString(d)+",h:"+(int)(h));

                    if(end == index) {
                        Log.i(Constants.TAG, "找到终点");
                        find = true;
                        break;
                    }
                } else {
                    double gValue = curNode.gValue + getDistance(father, current);
                    if(gValue - open.get(inOpenIndex).gValue < 0.000001) {
                        Log.i(Constants.TAG,"找到更优路径");
                        Node nd = open.get(inOpenIndex);
                        nd.father = curNode.current;
                        nd.fatherIndex = curNode.index;
                        nd.setGValue(gValue);
                        Log.i(Constants.TAG,"改变父结点为"+index+",重设g为:"+Double.toString(gValue));
                        //nd.setHValue(hValue);
                    }
                }
            }

            if(open.isEmpty()) {
                Log.i(Constants.TAG, "未找到终点");
                break;
            }
        } while(!find);

        if(!find) {
            Log.i(Constants.TAG, "未找到路径");
            return null;
        } else {
            List<Integer> result = new ArrayList<Integer>();
            result.add(end);
            Node node = open.get(open.size()-1);
            while(node.father != null) {
                Log.i(Constants.TAG, "找到路径点" + node.fatherIndex);
                result.add(node.fatherIndex);
                for(int i=0; i<close.size(); i++) {
                    Node nd = close.get(i);
                    if(nd.index == node.fatherIndex) {
                        node = nd;
                        break;
                    }
                }
            }
            return result;
        }
    }

    public static double getDistance(Point point1,Point point2){
        double d = (point2.x-point1.x)*(point2.x-point1.x) + (point2.y-point1.y)*(point2.y-point1.y);
        return Math.sqrt(d);
    }

    public static double getHuffmanDistance(Point point1,Point point2) {
        return Math.abs(point2.x-point1.x) + Math.abs(point2.y-point1.y);
    }

    private class Node {
        public Point father;
        public int fatherIndex;
        public Point current;
        public int index;
        public double gValue;
        public double hValue;

        public Node(Point father,int fatherIndex,Point currnet,int index) {
            this.father = father;
            this.current = currnet;
            this.index = index;
            this.fatherIndex = fatherIndex;
            gValue = 0;
            hValue = 0;
        }

        public void setGValue(double value) {
            gValue = value;
        }

        public void addGValue(double value) {
            gValue += value;
        }

        public void setHValue(double value) {
            hValue = value;
        }
    }
}